#include "tlsf.h"

void _tlsf::insert(_block *pbl) {
	assert(pbl->free());

	int sz = pbl->get_size();
//EnterCriticalSection(&indexing);
	int i = fls(sz);
	int j = (sz >> (i-SLI)) ^ (1 << SLI);
//EnterCriticalSection(&cs[i][j]);
//LeaveCriticalSection(&indexing);
	assert(i >= SLI && "inserting too small block into tlsf");

	LOG << "tlsf insert "  << i << ' ' << j << ' ' << pbl << std::endl;

	pbl->next_free = lists[i][j];
	pbl->pp_prev_next = &lists[i][j];
	pbl->i = i;
	pbl->j = j;
	if(lists[i][j]) {
		lists[i][j]->pp_prev_next = &(pbl->next_free);
	}
	lists[i][j] = pbl;
	L1 |= 1 << i;
	L2[i] |= 1 << j;
//LeaveCriticalSection(&cs[i][j]);
}

 _block* _tlsf::get(size_t sz) {
	sz += (1 << (fls(sz)-SLI)) - 1;

//EnterCriticalSection(&indexing);
	int i = fls(sz);
	int j = (sz >> (i-SLI)) ^ (1 << SLI);
	j += ffs(L2[i] >> j);
//EnterCriticalSection(&cs[i][j]);
//LeaveCriticalSection(&indexing);

	assert(i >= SLI && "finding too small block in tlsf");

	if(! (L2[i] >> j) ) {
		if(! (L1 >> i) ) {
			return NULL;
		}
		i += ffs(L1 >> i);
		j = ffs(L2[i]);
	}

	_block *t = lists[i][j];
	LOG << "tlsf get    "  << i << ' ' << j << ' ' << t << std::endl;
	assert(t);
	assert(t->free());
	lists[i][j] = t->next_free;
	if(lists[i][j]) {
		lists[i][j]->pp_prev_next = &lists[i][j];
	} else  {
		L2[i] ^= 1 << j;
		if(!L2[i])
			L1 ^= 1 << i;
	}
//LeaveCriticalSection(&cs[i][j]);
	return t;
}

void _tlsf::erase(_block *pbl) {
	LOG << "tlsf erase  " << pbl->i << ' ' << pbl->j << ' ' << pbl << std::endl;
	int i = pbl->i;
	int j = pbl->j;

	assert(pbl->free() && "erasing not free block");

	assert(*(pbl->pp_prev_next) == pbl);

	if(pbl->next_free)
		pbl->next_free->pp_prev_next = pbl->pp_prev_next;
	*(pbl->pp_prev_next) = pbl->next_free;


	if( ! lists[pbl->i][pbl->j] ) {
		L2[pbl->i] ^= 1 << pbl->j;
		if(!L2[pbl->i])
			L1 ^= 1 << pbl->i;
	}
}
